/******************************************************************************
 * 用Stanley算法实现横向轨迹跟踪，用PID实现纵向速度控制.
 * @author:HuYunhao
 * @Ref:
 *  - [Stanley: The robot that won the DARPA grand challenge](http://isl.ecst.csuchico.edu/DOCS/darpa2005/DARPA%202005%20Stanley.pdf)
    - [Autonomous Automobile Path Tracking](https://www.ri.cmu.edu/pub_files/2009/2/Automatic_Steering_Methods_for_Autonomous_Automobile_Path_Tracking.pdf)
 ******************************************************************************/

#ifndef CPP_STANLEY_CONTROLLER_H
#define CPP_STANLEY_CONTROLLER_H

#include <iostream>
#include <fstream>
#include "my_functions.h"
#include "nlohmann/json.hpp"

namespace controlNS {
struct StanleyParm{
    float pi{myNumpy::kPI};           // 定义pi
    float stanley_gain{1.0f};         // stanley控制增益参数
    float heading_gain{1.0f};         // heading_error增益参数
    float time_step{0.01f};           // 仿真时间间隔[s]
    float wheel_base{2.8f};           // 车辆轴距[m]
    float steer_angle_limit{30.0f};   // 车轮最大转角限制[deg]
    float steer_ratio{15.0f};         // 转向传动比
};

class State {
    /**用来表示车辆状态的类*/
public:
    explicit State(float x = 0, float y = 0, float yaw = 0, float v = 0);
    void UpdateForPreScan(float x, float y, float yaw, float v);
public:
    float x_;   //车辆的全局x坐标[m]
    float y_;   //车辆的全局y坐标[m]
    float yaw_; //车辆的横摆角[rad]
    float v_;   //车辆的全局速度[m/s]

};

class StanleyController {
    /**基于Stanley理论的横向控制器*/
public:
    StanleyController();
    ~StanleyController();
    void getVehicleStates(const State& state);  // 获取车辆的坐标[fx_,fy_,yaw_,v_]
    void getPathStates();                       // 获取规划路径[cx_,cy_,cyaw_]
    void calcNearestIndex();
    void calcError();
    float calcDelta(const State& state);
    float makeDeltaSmooth(float delta);    // 对转角进行平滑和滤波

public:
    float fx_{0};                          // 车辆前轮中心的x坐标[m]
    float fy_{0};                          // 车辆前轮中心的y坐标[m]
    float yaw_{0};                         // 车辆的yaw angle[rad]
    float v_{0};                           // 车辆实际车速[m/s]
    int min_distance_index_{0};            // 参考点的索引
    float cross_track_error_{0};           // 横向偏差[m]
    float heading_error_{0};               // 航向偏差[rad]
    float delta_{0};                       // stanley算法计算出的车轮转角[rad]
    std::vector<float> cx_;                     // 规划路径的x坐标[m];
    std::vector<float> cy_;                     // 规划路径的y坐标[m];
    std::vector<float> cyaw_;                   // 规划路径的切线角[rad];
    std::fstream json_file;                // 读取.json为stream流
    nlohmann::json control_json;           // 将stream流解析为json格式
    StanleyParm parm_;                     // 用于存放车辆的基本参数，stanley的控制参数

};

};  //namespace controlNS
#endif //CPP_STANLEY_CONTROLLER_H
